#ifndef ROBOTCMDNODE_ROBOTSERVER_PC_H_
#define ROBOTCMDNODE_ROBOTSERVER_PC_H_

/*****************************************************************************
** Includes
*****************************************************************************/
// ROS2
#include "clawcmd_msg/msg/claw_cmd.hpp"
#include "param_msg/msg/param.hpp"
#include "pccmd_msg/msg/pc_cmd.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rspos_msg/msg/rs_pos.hpp"
#include "rsswitch_msg/msg/rs_switch.hpp"
#include "slampos_msg/msg/slam_pos.hpp"
#include "slamready_msg/msg/slam_ready.hpp"

// CPP
#include <chrono>
#include <functional>
#include <memory>
#include <string>

class RobotServerPC;

/*****************************************************************************
 ** Class
 *****************************************************************************/

class RobotCmdNode : public rclcpp::Node {
public:
    RobotCmdNode(RobotServerPC* rspc);
    virtual ~RobotCmdNode();

    void sendCmd(const std::string& cmdname, const std::string& cmdcontent);
    // nouse
    void clawControl(uint8_t claw_dir, uint8_t claw_force);

    void rsswitch(bool b);

    void setSendRsPos(float xl,
                      float yl,
                      float zl,
                      float ll,
                      float wl,
                      float hl,
                      float xr,
                      float yr,
                      float zr,
                      float lr,
                      float wr,
                      float hr,
                      std::string serial);

private:
    /*********************
     ** Subscriber y
     ** Publisher
     **********************/
    rclcpp::Publisher<pccmd_msg::msg::PcCmd>::SharedPtr m_pubCmd;

    rclcpp::Publisher<clawcmd_msg::msg::ClawCmd>::SharedPtr m_pubClawCmd;

    rclcpp::Publisher<rsswitch_msg::msg::RsSwitch>::SharedPtr m_pubRsSwitch;

    rclcpp::Publisher<rspos_msg::msg::RsPos>::SharedPtr m_pubRsPos;

    // subs
    rclcpp::Subscription<slamready_msg::msg::SlamReady>::SharedPtr
        m_subSlamReady;

    // rclcpp::Subscription<param_msg::msg::Param>::SharedPtr m_subParam;

    rclcpp::Subscription<slampos_msg::msg::SlamPos>::SharedPtr m_subSlamPos;

    // void parammsg_callback(const param_msg::msg::Param::SharedPtr msg) const;

    void slamready_callback(
        const slamready_msg::msg::SlamReady::SharedPtr msg) const;

    void slampos_callback(const slampos_msg::msg::SlamPos::SharedPtr msg) const;

    RobotServerPC* m_rspc;
};

#endif /* ROBOTCMDNODE_ROBOTSERVER_PC_H_ */
